import math
from ctypes import *

import numpy as np
import numpy.ctypeslib as npct
from numpy.core.defchararray import index
from numpy.core.records import record
import matplotlib.pyplot as plt

#label  index  black 1  green  2 white  3
file = open("/home/st/ubuntu_data/exp_record/beijing/0218/brown_gps_gt2.txt", "r")
tra_list = file.readlines()
brown_gt = []
for i in range(len(tra_list)):
    brown_gt.append(np.array(tra_list[i].strip().split(' '), dtype = np.float64 ))
brown_gt = np.array(brown_gt)
file.close()

#read data record
file = open("/home/st/ubuntu_data/exp_record/beijing/0218/2222.txt", "r")
tra_list = file.readlines()
record_data = []
for i in range(len(tra_list)):
    record_data.append(np.array(tra_list[i].strip().split(' '), dtype = np.float64 ))
record_data = np.array(record_data)
file.close()

#按照类别将数据进行划分
brown_mea = []
white_mea = []
for i in range(record_data.shape[0]):
    if record_data[i, 1] == 0:
        white_mea.append(record_data[i, :])
    if record_data[i, 1] == 1:
        brown_mea.append(record_data[i, :])
brown_mea = np.array(brown_mea)
brown_mea = brown_mea[30:-35, :]

#与gps数据对齐
brown_gt[:,0] = brown_gt[:,0]+1
brown_gt[:,1] = brown_gt[:,1]-0.0

#kf
from filterpy.kalman import KalmanFilter
kf_gt = KalmanFilter(dim_x=6, dim_z=2)
delta_t = brown_gt[1,0] - brown_gt[0,0]
kf_gt.F = np.array([[1,0,delta_t,0,0.5*delta_t**2,0],      # state transition matrix
                    [0,1,0,delta_t,0,0.5*delta_t**2],
                    [0,0,1,0,delta_t,0],
                    [0,0,0,1,0,delta_t],
                    [0,0,0,0,1,0],
                    [0,0,0,0,0,1]])
kf_gt.H = np.array([[1,0,0,0,0,0],
                    [0,1,0,0,0,0]])
kf_gt.P[2:, 2:] *= 0.1	# state uncertainty, give high uncertainty to the unobservable initial velocities, covariance matrix
kf_gt.P *= 0.1
kf_gt.Q[0:, 0:] *= 0.01
kf_gt.R = kf_gt.R*0.8
kf_gt.x = np.array([brown_gt[0,1],brown_gt[0,2],0,0,0,0])
for i in range(brown_gt.shape[0]):
    kf_gt.predict()
    kf_gt.update(brown_gt[i,1:3])
    brown_gt[i,8:10] = kf_gt.x[2:4]

brown_gt = brown_gt[200:-227, :]

#pos error
def pos_error_fun(black_mea, black_gt):
    pos_error_radio_black = []
    for i in range(black_mea.shape[0]):
        index = np.where(black_gt[:, 0] > black_mea[i, 0] )
        index_front = index[0][0] - 1
        index_back = index[0][0]
        ratio = (black_mea[i, 0] - black_gt[index_front, 0])/(black_gt[index_back, 0]  -black_gt[index_front, 0] )
        pos_interval_x =  ratio*(black_gt[index_back, 1]  - black_gt[index_front, 1] ) + black_gt[index_front, 1]
        pos_interval_y =  ratio*(black_gt[index_back, 2]  - black_gt[index_front, 2] ) + black_gt[index_front, 2]
        # pos_interval_z =  ratio*(black_gt[index_back, 3]  - black_gt[index_front, 3] ) + black_gt[index_front, 3]

        # error = (pos_interval_x - black_mea[i, 2])**2 + (pos_interval_y - black_mea[i, 3])**2  + (pos_interval_z - black_mea[i, 4] + 0.3)**2  #z pos bias
        error = (pos_interval_x - black_mea[i, 2])**2 + (pos_interval_y - black_mea[i, 3])**2
        error = math.sqrt(error)

        # index = np.where(master_gt[:, 0] > black_mea[i, 0] )
        # index_front = index[0][0] - 1
        # index_back = index[0][0]
        # ratio = (black_mea[i, 0] - master_gt[index_front, 0])/(master_gt[index_back, 0]  -master_gt[index_front, 0] )
        # master_pos_interval_x =  ratio*(master_gt[index_back, 1]  - master_gt[index_front, 1] ) + master_gt[index_front, 1]
        # master_pos_interval_y =  ratio*(master_gt[index_back, 2]  - master_gt[index_front, 2] ) + master_gt[index_front, 2]
        # master_pos_interval_z =  ratio*(master_gt[index_back, 3]  - master_gt[index_front, 3] ) + master_gt[index_front, 3]

        # distance_gt = (pos_interval_x - master_pos_interval_x)**2 + (pos_interval_y - master_pos_interval_y)**2  + (pos_interval_z - master_pos_interval_z + 0.1251)**2  #z pos bias
        # distance_gt = (pos_interval_x - master_pos_interval_x)**2 + (pos_interval_y - master_pos_interval_y)**2
        distance_gt = (pos_interval_x)**2 + (pos_interval_y)**2
        distance_gt =math.sqrt(distance_gt)
        pos_error_radio_black.append(float(error/distance_gt))
    return np.array(pos_error_radio_black)

#pos
pos_error_radio_black = pos_error_fun(brown_mea, brown_gt)
print("对抗目标  相对位置误差： ", np.mean(pos_error_radio_black))

#vel error
def vel_error_fun(black_mea, black_gt):
    vel_error_radio_black = []
    for i in range(0, black_mea.shape[0]-1):
        index = np.where(black_gt[:, 0] > black_mea[i, 0] )
        index_front = index[0][0] - 1
        index_back = index[0][0]
        ratio = (black_mea[i, 0] - black_gt[index_front, 0])/(black_gt[index_back, 0]  -black_gt[index_front, 0] )
        vel_interval_x =  ratio*(black_gt[index_back, 8]  - black_gt[index_front, 8] ) + black_gt[index_front, 8]
        vel_interval_y =  ratio*(black_gt[index_back, 9]  - black_gt[index_front, 9] ) + black_gt[index_front, 9]
        # vel_interval_z =  ratio*(black_gt[index_back, 10]  - black_gt[index_front, 10] ) + black_gt[index_front, 10]

        # error = (vel_interval_x - black_mea[i, 5])**2 + (vel_interval_y - black_mea[i, 6])**2  + (vel_interval_z - black_mea[i, 7] )**2
        error = (vel_interval_x - black_mea[i, 5])**2 + (vel_interval_y - black_mea[i, 6])**2
        error = math.sqrt(error)

        # index = np.where(master_gt[:, 0] > black_mea[i, 0] )
        # index_front = index[0][0] - 1
        # index_back = index[0][0]
        # ratio = (black_mea[i, 0] - master_gt[index_front, 0])/(master_gt[index_back, 0]  -master_gt[index_front, 0] )
        # master_vel_interval_x =  ratio*(master_gt[index_back, 8]  - master_gt[index_front, 8] ) + master_gt[index_front, 8]
        # master_vel_interval_y =  ratio*(master_gt[index_back, 9]  - master_gt[index_front, 9] ) + master_gt[index_front, 9]
        # master_vel_interval_z =  ratio*(master_gt[index_back, 10]  - master_gt[index_front, 10] ) + master_gt[index_front, 10]
        #
        # vel_gt = (vel_interval_x - master_vel_interval_x)**2 + (vel_interval_y - master_vel_interval_y)**2  + (vel_interval_z - master_vel_interval_z)**2
        # vel_gt =math.sqrt(vel_gt)

        black_get_xyz = (vel_interval_x)**2 + (vel_interval_y )**2
        black_get_xyz = math.sqrt(black_get_xyz)

        if(black_get_xyz==0):
            continue
        vel_error_radio_black.append(float(error/black_get_xyz))

    return np.array(vel_error_radio_black)

#vel error
vel_error_radio_black = vel_error_fun(brown_mea, brown_gt)
print("对抗目标  相对速度误差： ", np.mean(vel_error_radio_black))

#pre pos error
def interval_gt(temp_t, temp_t_front, temp_t_back, temp_x_front, temp_x_back, temp_y_front, temp_y_back, temp_z_front, temp_z_back):
    ratio = (temp_t - temp_t_front)/(temp_t_back  - temp_t_front )
    vel_interval_x =  ratio*(temp_x_back  - temp_x_front ) + temp_x_front
    vel_interval_y =  ratio*(temp_y_back  - temp_y_front) + temp_y_front
    vel_interval_z =  ratio*(temp_z_back  - temp_z_front) + temp_z_front
    return vel_interval_x, vel_interval_y, vel_interval_z

def pre_error_fun(black_mea, black_gt):
    pos_error_radio_all = []
    for i in range(black_mea.shape[0]):
        time_stamp_base = black_mea[i, 0]

        # index = np.where(master_gt[:, 0] > time_stamp_base)
        # index_front = index[0][0] - 1
        # index_back = index[0][0]
        # master_pos_interval_gt_x, master_pos_interval_gt_y, master_pos_interval_gt_z = interval_gt(time_stamp_base, master_gt[index_front, 0], master_gt[index_back, 0],\
        #         master_gt[index_front, 1], master_gt[index_back, 1],\
        #         master_gt[index_front, 2], master_gt[index_back, 2],\
        #         master_gt[index_front, 3], master_gt[index_back, 3])

        index = np.where(black_gt[:, 0] > time_stamp_base)
        index_front = index[0][0] - 1
        index_back = index[0][0]
        pre_pos_interval_gt_x, pre_pos_interval_gt_y, pre_pos_interval_gt_z = interval_gt(time_stamp_base, black_gt[index_front, 0], black_gt[index_back, 0], \
                                                                                          black_gt[index_front, 1], black_gt[index_back, 1], \
                                                                                          black_gt[index_front, 2], black_gt[index_back, 2], \
                                                                                          black_gt[index_front, 3], black_gt[index_back, 3])
        cur_distance_gt = (pre_pos_interval_gt_x)**2 + (pre_pos_interval_gt_y)**2   #z pos bias
        cur_distance_gt =math.sqrt(cur_distance_gt)

        pos_error_radio = []
        for j in range(10):
            index = np.where(black_gt[:, 0] > time_stamp_base + (j+1)*0.1 )
            index_front = index[0][0] - 1
            index_back = index[0][0]
            pre_pos_interval_gt_x, pre_pos_interval_gt_y, pre_pos_interval_gt_z = interval_gt(time_stamp_base + (j+1)*0.1, black_gt[index_front, 0], black_gt[index_back, 0], \
                                                                                              black_gt[index_front, 1], black_gt[index_back, 1], \
                                                                                              black_gt[index_front, 2], black_gt[index_back, 2], \
                                                                                              black_gt[index_front, 3], black_gt[index_back, 3])

            error = (pre_pos_interval_gt_x - black_mea[i, 8 + j*3])**2 + (pre_pos_interval_gt_y - black_mea[i, 9 + j*3])**2  #z pos bias
            error = math.sqrt(error)

            pos_error_radio.append(float(error/cur_distance_gt))

        pos_error_radio = np.array(pos_error_radio)
        pos_error_radio_all.append(np.mean(pos_error_radio))

    return np.array(pos_error_radio_all)

#pre pos error
pre_error_radio_black = pre_error_fun(brown_mea, brown_gt)
print("对抗目标  轨迹预测误差： ", np.mean(pre_error_radio_black))

data_length = min([pos_error_radio_black.shape[0],vel_error_radio_black.shape[0],pre_error_radio_black.shape[0]])
success = 0
for i in range(data_length):
    if pos_error_radio_black[i] < 0.15 and vel_error_radio_black[i]<0.25:
        success += 1

print("对抗目标 成功率： ",success/data_length,"时长： ",data_length*0.1,"s")

brown_gt_up = brown_gt[:,8] + np.abs(brown_gt[:,8])*0.25
brown_gt_down = brown_gt[:,8] - np.abs(brown_gt[:,8])*0.25


plt.figure(1)
l1, = plt.plot(brown_mea[:,0], brown_mea[:,5], 'b') #v x
l2, = plt.plot(brown_gt[:,0], brown_gt[:,8], 'r-.') #v x
plt.legend(handles=[l1,l2],labels=['Vx_estimated','Vx_gt'])

plt.xlabel('t(s)')
plt.ylabel('Vx(m/s)')

plt.fill_between(brown_gt[:,0],brown_gt_up,brown_gt_down,facecolor='green', edgecolor='red', alpha=0.3)


# plt.plot(brown_mea[:,0], brown_mea[:,6], 'b') #v y
# plt.plot(brown_gt[:,0], brown_gt[:,9], 'r-.') #v y
# plt.show()

brown_gt_pos_up = brown_gt[:,1] + np.abs(brown_gt[:,1])*0.15
brown_gt_pos_down = brown_gt[:,1] - np.abs(brown_gt[:,1])*0.15

plt.figure(2)
l1,=plt.plot(brown_mea[:,0], brown_mea[:,2], 'b') #x
l2,=plt.plot(brown_gt[:,0], brown_gt[:,1], 'r-.') #x
plt.legend(handles=[l1,l2],labels=['x_estimated','x_gt'],loc="best")

plt.xlabel('t(s)')
plt.ylabel('x(m)')

plt.fill_between(brown_gt[:,0],brown_gt_pos_up,brown_gt_pos_down,facecolor='green', edgecolor='red', alpha=0.3)

plt.show()

# plt.plot(brown_mea[:,0], brown_mea[:,3], 'b') #y
# plt.plot(brown_gt[:,0], brown_gt[:,2], 'r-.') #y
# plt.show()
plt.show()